#include "config.h" /* LINELEN definition */
#include <stdlib.h> /* exit() */
#include <sys/stat.h>
#include <string.h>            /* memcpy() */
#include <float.h>             /* DBL_MIN */
#include "motion.h"            /* zucmot_status_t,CMD */
#include "motion_debug.h"      /* zucmot_debug_t */
#include "motion_struct.h"     /* zucmot_struct_t */
#include "zucmotcfg.h"         /* ZUCMOT_ERROR_NUM,LEN */
#include "zucmotglb.h"         /* SHMEM_KEY */
#include "motion/usrmotintf.h" /* these decls */
#include "_timer.h"
#include "rcs_print.hh"

// #include "inifile.hh"
// #include "zucIniFile.hh"
#include "jkutil/iniparser.h"

#define READ_TIMEOUT_SEC 0       /* seconds for timeout */
#define READ_TIMEOUT_USEC 100000 /* microseconds for timeout */

#include "rtapi/rtapi.h"

#include "dbuf.h"
#include "stashf.h"
#include <unistd.h>

static int inited = 0; /* flag if inited */

static zucmot_command_t* zucmotCommand = 0;
static zucmot_status_t* zucmotStatus = 0;
static zucmot_config_t* zucmotConfig = 0;
static zucmot_debug_t* zucmotDebug = 0;
static zucmot_error_t* zucmotError = 0;
static struct timespec sync_ts = {0, 500000};

/* writes command from c */
int usrmotWriteZucmotCommand(zucmot_command_t* c)
{
    zucmot_status_t s;
    static int commandNum = 0;
    static unsigned char headCount = 0;
    double end;

    if (!MOTION_ID_VALID(c->id))
    {
        rcs_print("USRMOT: ERROR: invalid motion id: %d\n", c->id);
        return ZUCMOT_COMM_INVALID_MOTION_ID;
    }
    c->head = ++headCount;
    c->tail = c->head;
    *zucmotCommand = *c;
    c->commandNum = ++commandNum;

    /* check for mapped mem still around */
    if (0 == zucmotCommand)
    {
        rcs_print("USRMOT: ERROR: can't connect to shared memory\n");
        return ZUCMOT_COMM_ERROR_CONNECT;
    }

    // if(ZUCMOT_SET_ROBOT_TOOL_OFFSET==c->command) {
    // 	rtapi_print("task set tooloffset no %d: tool_offset = %f %f %f %f %f %f \n ", c->commandNum,
    // 	c->trajsetrobtooloffset.tool_offset.tran.x, c->trajsetrobtooloffset.tool_offset.tran.y, c->trajsetrobtooloffset.tool_offset.tran.z,
    // 	c->trajsetrobtooloffset.tool_offset.a, c->trajsetrobtooloffset.tool_offset.b, c->trajsetrobtooloffset.tool_offset.c);
    // }

    /* copy entire command structure to shared memory */
    *zucmotCommand = *c;
    /* poll for receipt of command */
    /* set timeout for comm failure, now + timeout */
    end = etime() + 1.0;
    /* now check to see if it got it */
    while (etime() < end)
    {
        /* update status */
        if ((usrmotReadZucmotStatus(&s) == 0) && (s.commandNumEcho == commandNum))
        {
            /* now check zucmot status flag */
            if (s.commandStatus == ZUCMOT_COMMAND_OK)
            {
                return ZUCMOT_COMM_OK;
            }
            else
            {
                rcs_print("USRMOT: ERROR: invalid command\n");
                return ZUCMOT_COMM_ERROR_COMMAND;
            }
        }
        esleep(25e-6);
    }
    rcs_print("USRMOT: ERROR: command timeout\n");
    return ZUCMOT_COMM_ERROR_TIMEOUT;
}

/* copies status to s */
int usrmotReadZucmotStatus(zucmot_status_t* s)
{
    int split_read_count;

    /* check for shmem still around */
    if (0 == zucmotStatus)
    {
        return ZUCMOT_COMM_ERROR_CONNECT;
    }
    split_read_count = 0;
    do {
        /* copy status struct from shmem to local memory */
        memcpy(s, zucmotStatus, (int)sizeof(zucmot_status_t));
        /* got it, now check head-tail matche */
        if (s->head == s->tail)
        {
            /* head and tail match, done */
            return ZUCMOT_COMM_OK;
        }
        usleep(sync_ts.tv_nsec / 1000);
        // nanosleep(&sync_ts, NULL);
        /* inc counter and try again, max 8 times */
    } while (++split_read_count < 8);
    // rtapi_print("USRMOT:usrmotReadZucmotStatus split read timeout\n");
    return ZUCMOT_COMM_SPLIT_READ_TIMEOUT;
}

/* copies config to s */
int usrmotReadZucmotConfig(zucmot_config_t* s)
{
    return 0;
    int split_read_count;

    /* check for shmem still around */
    if (0 == zucmotConfig)
    {
        return ZUCMOT_COMM_ERROR_CONNECT;
    }
    split_read_count = 0;
    do {
        memcpy(s, zucmotConfig, (int)sizeof(zucmot_config_t));
        if (s->head == s->tail)
        {
            return ZUCMOT_COMM_OK;
        }
        usleep(sync_ts.tv_nsec / 1000);
        // nanosleep(&sync_ts, NULL);
    } while (++split_read_count < 3);
    return ZUCMOT_COMM_SPLIT_READ_TIMEOUT;
}

/* copies debug to s */
int usrmotReadZucmotDebug(zucmot_debug_t* s)
{
    //only copy required info
    if (0 == zucmotDebug)
    {
        return ZUCMOT_COMM_ERROR_CONNECT;
    }
    s->stepping = zucmotDebug->stepping;
    s->idForStep = zucmotDebug->idForStep;
    return ZUCMOT_COMM_OK;

    // int split_read_count;
    // if (0 == zucmotDebug) {
    // 	return ZUCMOT_COMM_ERROR_CONNECT;
    // }
    // split_read_count = 0;
    // do {
    // 	memcpy(s, zucmotDebug,(int)sizeof(zucmot_debug_t));
    // 	if (s->head == s->tail) {
    // 		return ZUCMOT_COMM_OK;
    // 	}
    // } while ( ++split_read_count < 3 );
    // return ZUCMOT_COMM_SPLIT_READ_TIMEOUT;
}

/* copies error to s */
int usrmotReadZucmotError(unsigned char* type, int* perrcode, char* e)
{
    /* check to see if ptr still around */
    if (zucmotError == 0)
    {
        return -1;
    }

    char data[ZUCMOT_ERROR_LEN];
    struct dbuf d;
    dbuf_init(&d, (unsigned char*)data, ZUCMOT_ERROR_LEN);

    /* returns 0 if something, -1 if not */
    int result = zucmotErrorGet(zucmotError, type, perrcode, data);
    if (result < 0)
        return result;

    struct dbuf_iter di;
    dbuf_iter_init(&di, &d);

    result = snprintdbuf(e, ZUCMOT_ERROR_LEN, &di);
    if (result < 0)
        return result;
    return 0;
}

/*
 htostr()

 converts short int to 0-1 style string, in s. Assumes a short is 2 bytes.
*/
/*! \todo Another #if 0 */
#if 0 /*! \todo FIXME - don't know if this is still needed 
				 */

static int htostr(char *s, unsigned short h)
{
    int t;

    for (t = 15; t >= 0; t--) {
	s[t] = h % 2 ? '1' : '0';
	h >>= 1;
    }
    s[16] = 0;			/* null terminate */

    return 0;
}
#endif

void usrmotPrintZucmotDebug(zucmot_debug_t* d, int which)
{
    //    int t;

    printf("running time: \t%f\n", d->running_time);
    switch (which)
    {
/*! \todo Another #if 0 */
#if 0
	printf("\nferror:        ");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", d->ferrorCurrent[t]);
	}
	printf("\n");

	printf("\nferror High:        ");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", d->ferrorHighMark[t]);
	}
	printf("\n");
	break;
    case 5:
	printf("traj  m/m/a:\t%f\t%f\t%f\n", d->tMin, d->tMax, d->tAvg);
	printf("\n");
	printf("servo m/m/a:\t%f\t%f\t%f\n", d->sMin, d->sMax, d->sAvg);
	printf("\n");
	printf("(off) m/m/a:\t%f\t%f\t%f\n", d->nMin, d->nMax, d->nAvg);
	printf("\n");
	printf("(cycle to cycle  time) m/m/a:\t%f\t%f\t%f\n", d->yMin, d->yMax,
	    d->yAvg);
	printf("\n");
	printf("(frequency compute  time) m/m/a:\t%f\t%f\t%f\n", d->fMin,
	    d->fMax, d->fAvg);
	printf("\n");
	printf("(frequecy cycle to cycle  time) m/m/a:\t%f\t%f\t%f\n",
	    d->fyMin, d->fyMax, d->fyAvg);
	printf("\n");
	break;
#endif

    case 6:
    case 7:
    case 8:
    case 9:
    case 10:
    case 11:
//      printf("jointPos[%d]: %f\n", which - 6, d->jointPos[(which - 6)]);
/*! \todo Another #if 0 */
#if 0 /*! \todo FIXME - change to work with joint
				   structures */
	printf("coarseJointPos[%d]: %f\n",
	    which - 6, d->coarseJointPos[(which - 6)]);
	printf("jointVel[%d]: %f\n", which - 6, d->jointVel[(which - 6)]);
	printf("rawInput[%d]: %f\n", which - 6, d->rawInput[(which - 6)]);
	printf("rawOutput[%d]: %f\n", which - 6, d->rawOutput[(which - 6)]);
#endif
        //      printf("bcompincr[%d]: %f\n", which - 6, d->bcompincr[(which - 6)]);
        break;

    case 12:
/*! \todo Another #if 0 */
#if 0 /*! \todo FIXME - change to work with joint
				   structures */
	printf("\noldInput:  ");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", d->oldInput[t]);
	}
	printf("\nrawInput:  ");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", d->rawInput[t]);
	}
	printf("\ninverseInputScale:  ");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", d->inverseInputScale[t]);
	}
#endif
        printf("\n");

    default:
        break;
    }
}

void usrmotPrintZucmotConfig(zucmot_config_t c, int which)
{
    //    int t;
    //    char m[32];

    switch (which)
    {
    case 0:
        printf("debug level   \t%d\n", c.debug);
        printf("traj time:    \t%f\n", c.trajCycleTime);
        printf("servo time:   \t%f\n", c.servoCycleTime);
        printf("interp rate:  \t%d\n", c.interpolationRate);
        printf("v limit:      \t%f\n", c.limitVel);
        printf("axis vlimit:  \t");
/*! \todo Another #if 0 */
#if 0 /*! \todo FIXME - waiting for new structs */
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("%f ", c.axisLimitVel[t]);
	}
	printf("\n");
	printf("axis acc: \t");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("%f ", c.axisLimitAcc[t]);
	}
	printf("\n");
#endif
        printf("\n");
        break;

    case 1:
        printf("pid stuff is obsolete\n");
/*! \todo Another #if 0 */
#if 0
	printf
	    ("pid:\tP\tI\tD\tFF0\tFF1\tFF2\tBCKLSH\tBIAS\tMAXI\tDEADBAND\tCYCLE TIME\n");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf
		("\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%f\t%f\n",
		c.pid[t].p, c.pid[t].i, c.pid[t].d, c.pid[t].ff0,
		c.pid[t].ff1, c.pid[t].ff2, c.pid[t].backlash, c.pid[t].bias,
		c.pid[t].maxError, c.pid[t].deadband, c.pid[t].cycleTime);
	}
	printf("\n");
#endif
        break;

    case 3:
/*! \todo Another #if 0 */
#if 0 /*! \todo FIXME - waiting for new structs */
	printf("pos limits:   ");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", c.maxLimit[t]);
	}

	printf("\nneg limits:   ");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", c.minLimit[t]);
	}

	printf("\nmax ferror:   ");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", c.maxFerror[t]);
	}
	printf("\n");

	printf("\nmin ferror:   ");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", c.minFerror[t]);
	}
	printf("\n");

	printf("\nhome offsets:  ");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", c.homeOffset[t]);
	}
	printf("\n");
#endif
        break;

    default:
        break;
    }
}

/* status printing function */
void usrmotPrintZucmotStatus(zucmot_status_t* s, int which)
{
    //    int t;
    //    char m[32];

    switch (which)
    {
    case 0:
        printf("mode:         \t%s\n",
               s->motionFlag & ZUCMOT_MOTION_SERVOJOP_BIT ?
                   "servoj" :
                   (s->motionFlag & ZUCMOT_MOTION_DRAG_BIT ?
                        "drag" :
                        (s->motionFlag & ZUCMOT_MOTION_TELEOP_BIT ?
                             "teleop" :
                             (s->motionFlag & ZUCMOT_MOTION_TELEOP_TOOL_BIT ? "teleop_tool" : (s->motionFlag & ZUCMOT_MOTION_COORD_BIT ? "coord" : "free")))));
        printf("cmd:          \t%d\n", s->commandEcho);
        printf("cmd num:      \t%d\n", s->commandNumEcho);
        printf("heartbeat:    \t%u\n", s->heartbeat);
/*! \todo Another #if 0 */
#if 0 /*! \todo FIXME - change to work with joint
				   structures */
	printf("axes enabled: \t");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("%d", s->axisFlag[t] & ZUCMOT_JOINT_ENABLE_BIT ? 1 : 0);
	}
	printf("\n");
#endif
        printf("cmd pos:      \t%f\t%f\t%f\t%f\t%f\t%f\n",
               s->carte_pos_cmd.tran.x,
               s->carte_pos_cmd.tran.y,
               s->carte_pos_cmd.tran.z,
               s->carte_pos_cmd.a,
               s->carte_pos_cmd.b,
               s->carte_pos_cmd.c);
        printf("act pos:      \t%f\t%f\t%f\t%f\t%f\t%f\n",
               s->carte_pos_fb.tran.x,
               s->carte_pos_fb.tran.y,
               s->carte_pos_fb.tran.z,
               s->carte_pos_fb.a,
               s->carte_pos_fb.b,
               s->carte_pos_fb.c);
        printf("joint data:\n");
/*! \todo Another #if 0 */
#if 0 /*! \todo FIXME - change to work with joint
				   structures */
	printf(" cmd: ");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", s->joint_pos_cmd[t]);
	}
	printf("\n");
	printf(" fb:  ");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", s->joint_pos_fb[t]);
	}
	printf("\n");
	printf(" vel: ");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", s->joint_vel_cmd[t]);
	}
	printf("\n");
	printf(" ferr:");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", s->ferrorCurrent[t]);
	}
	printf("\n");
	printf(" lim:");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", s->ferrorLimit[t]);
	}
	printf("\n");
	printf(" max:");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", s->ferrorHighMark[t]);
	}
	printf("\n");
#endif
        printf("velocity:     \t%f\n", s->vel);
        printf("accel:        \t%f\n", s->acc);
        printf("id:           \t%d\n", s->id);
        printf("depth:        \t%d\n", s->depth);
        printf("active depth: \t%d\n", s->activeDepth);
        printf("inpos:        \t%d\n", s->motionFlag & ZUCMOT_MOTION_INPOS_BIT ? 1 : 0);
/*! \todo Another #if 0 */
#if 0 /*! \todo FIXME - change to work with joint
				   structures */
	printf("vscales:      \tQ: %.2f", s->qVscale);
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%d: %.2f", t, s->axVscale[t]);
	}
	printf("\n");
#endif
/*! \todo Another #if 0 */
#if 0 /*! \todo FIXME - change to work with joint
				   structures */
	printf("homing:       \t");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("%d", s->axisFlag[0] & ZUCMOT_JOINT_HOMING_BIT ? 1 : 0);
	}
	printf("\n");
#endif
        printf("enabled:     \t%s\n", s->motionFlag & ZUCMOT_MOTION_ENABLE_BIT ? "ENABLED" : "DISABLED");
        break;

    case 2:
        /* print motion and axis flags */
/*! \todo Another #if 0 */
#if 0 /*! \todo FIXME - change to work with joint
				   structures */
	htostr(m, s->motionFlag);
	printf("motion:   %s\n", m);
	printf("axes:     ");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    htostr(m, s->axisFlag[t]);
	    printf("%s ", m);
	}
	printf("\n");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("%d\t", ((s->axisFlag[t] & ZUCMOT_JOINT_ENABLE_BIT) != 0));
	}
	printf("enable\n");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("%d\t", ((s->axisFlag[t] & ZUCMOT_JOINT_ACTIVE_BIT) != 0));
	}
	printf("active\n");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("%d\t", ((s->axisFlag[t] & ZUCMOT_JOINT_INPOS_BIT) != 0));
	}
	printf("inpos\n");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("%d\t", ((s->axisFlag[t] & ZUCMOT_JOINT_ERROR_BIT) != 0));
	}
	printf("error\n");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("%d\t",
		((s->axisFlag[t] & ZUCMOT_JOINT_MAX_SOFT_LIMIT_BIT) != 0));
	}
	printf("max_soft_limit\n");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("%d\t",
		((s->axisFlag[t] & ZUCMOT_JOINT_MIN_SOFT_LIMIT_BIT) != 0));
	}
	printf("min_soft_limit\n");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("%d\t",
		((s->axisFlag[t] & ZUCMOT_JOINT_MAX_HARD_LIMIT_BIT) != 0));
	}
	printf("max_hard_limit\n");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("%d\t",
		((s->axisFlag[t] & ZUCMOT_JOINT_MIN_HARD_LIMIT_BIT) != 0));
	}
	printf("min_hard_limit\n");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("%d\t",
		((s->axisFlag[t] & ZUCMOT_JOINT_HOME_SWITCH_BIT) != 0));
	}
	printf("home_switch\n");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("%d\t", ((s->axisFlag[t] & ZUCMOT_JOINT_HOMING_BIT) != 0));
	}
	printf("homing\n");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("%d\t", ((s->axisFlag[t] & ZUCMOT_JOINT_HOMED_BIT) != 0));
	}
	printf("homed\n");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("%d\t", ((s->axisFlag[t] & ZUCMOT_JOINT_FERROR_BIT) != 0));
	}
	printf("ferror\n");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("%d\t", ((s->axisFlag[t] & ZUCMOT_JOINT_FAULT_BIT) != 0));
	}
#endif
        printf("fault\n");
        printf("\npolarity: ");
        printf("limit override mask: %08x\n", s->overrideLimitMask);
        break;

    case 4:
        printf("scales handled in HAL now!\n");
/*! \todo Another #if 0 */
#if 0
	printf("output scales: ");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", s->outputScale[t]);
	}

	printf("\noutput offsets:");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", s->outputOffset[t]);
	}

	printf("\ninput scales:  ");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", s->inputScale[t]);
	}

	printf("\ninput offsets: ");
	for (t = 0; t < ZUCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", s->inputOffset[t]);
	}

	printf("\n");
#endif
        break;

    default:
        break;
    }
}

static int module_id;
static int shmem_id;

int usrmotInit(const char* modname)
{
    int retval;
    extern zucmot_struct_t* zucmotStruct;
    /* got it */
    zucmotCommand = &(zucmotStruct->command);
    zucmotStatus = &(zucmotStruct->status);
    zucmotDebug = &(zucmotStruct->debug);
    zucmotConfig = &(zucmotStruct->config);
    zucmotError = &(zucmotStruct->error);

    // ZucIniFile trajInifile;
    // trajInifile.Open(ZUC_USER_CONFIG_DIR "/usersettings.ini");
    // Util::IniParser ini_file(ZUC_USER_CONFIG_DIR "/usersettings.ini");
    // if (0 != trajInifile.Find(&zucmotConfig->cat1StopEffect.program_pause, "PROGRAMME_PAUSE", "CATEGORY_ONE_STOP"))
    // {
    //     rcs_print("read paramater CATEGORY_ONE_STOP:PROGRAMME_PAUSE failed \n");
    //     zucmotConfig->cat1StopEffect.program_pause = 0;
    // }
    // ini_file.get("CATEGORY_ONE_STOP.PROGRAMME_PAUSE", zucmotConfig->cat1StopEffect.program_pause, 0);
    // rcs_print("CATEGORY_ONE_STOP:PROGRAMME_PAUSE valuse is %s \n", zucmotConfig->cat1StopEffect.program_pause ? "True" : "False");
    // trajInifile.Close();

    inited = 1;

    return 0;
}

int usrmotExit(void)
{
    extern zucmot_struct_t* zucmotStruct;
    if (NULL != zucmotStruct)
    {
        rtapi_shmem_delete(shmem_id, module_id);
        rtapi_exit(module_id);
    }

    zucmotStruct = 0;
    zucmotCommand = 0;
    zucmotStatus = 0;
    zucmotError = 0;
/*! \todo Another #if 0 */
#if 0
/*! \todo FIXME - comp structs no longer in shmem */
    for (axis = 0; axis < ZUCMOT_MAX_JOINTS; axis++) {
	zucmotComp[axis] = 0;
    }
#endif

    inited = 0;
    return 0;
}

/* Loads pairs of comp from the compensation file.
   The default way is to specify nominal, forward & reverse triplets in the file
   However if type != 0, it expects nominal, forward_trim & reverse_trim 
	(where forward_trim = nominal - forward
	       reverse_trim = nominal - reverse)
*/
int usrmotLoadComp(int joint, const char* file, int type) { return 0; }

int usrmotPrintComp(int joint)
{
    /* FIXME-AJ: comp isn't in shmem atm
  it's in the joint struct, which is only in shmem when STRUCTS_IN_SHM is defined,
  currently only usrmot uses usrmotPrintComp - might go away */
    return -1;
}

int usrmotGetDIn(int type, int id, int* status)
{
    if (zucmotStatus == 0)
    {
        usrmotInit("zuc_task");
    }
    if (id < 0)
        return -1;
    if (type == 0)
    {
        if (id >= (int)sizeof(zucmotStatus->synch_di))
            return -1;
        *status = zucmotStatus->synch_di[id];
    }
    else if (type == 1)
    {
        if (id >= (int)sizeof(zucmotStatus->tio_status.DI_buffer) * 8)
            return -1;
        *status = (zucmotStatus->tio_status.DI_buffer & (1 << id)) > 0 ? 1 : 0;
    }
    else if (type == 2)
    {
        if (zucmotStatus->extio_stat.status != 1)
        {  // extio not running
            return -1;
        }
        if (id >= (int)sizeof(zucmotStatus->extio_stat.extDI))
            return -1;
        *status = zucmotStatus->extio_stat.extDI[id];
    }
    else if (type == 4)
    {
        if (id >= (int)sizeof(zucmotStatus->synch_mb_slave_di))
            return -1;
        *status = zucmotStatus->synch_mb_slave_di[id];
    }
    else if (type == 5)
    {
        if (id >= (int)sizeof(zucmotStatus->synch_pn_dev_di))
            return -1;
        *status = zucmotStatus->synch_pn_dev_di[id];
    }
    else if (type == 6)
    {
        if (zucmotStatus->eip_mode == 1 && id < zucmotStatus->eip_di_num)
        {
            int byteNum = id / 8;
            id -= byteNum * 8;
            *status = 0x01 & (0xff & (zucmotStatus->synch_eip_in[byteNum] >> id));
        }
        else if (zucmotStatus->eip_mode == 0 && id < (int)sizeof(zucmotStatus->synch_eip_adpt_di))
        {
            *status = zucmotStatus->synch_eip_adpt_di[id];
        }
        else
        {
            return -1;
        }
    }

    return 0;
}

int usrmotGetDOut(int type, int id, int* status)
{
    if (zucmotStatus == 0)
    {
        usrmotInit("zuc_task");
    }
    if (id < 0)
    {
        return -1;
    }
    if (type == 0)
    {
        if (id >= (int)sizeof(zucmotStatus->synch_do))
        {
            return -1;
        }
        *status = zucmotStatus->synch_do[id];
    }
    else if (type == 1)
    {
        if (id >= (int)sizeof(zucmotStatus->tio_status.DO_buffer) * 8)
        {
            return -1;
        }
        *status = (zucmotStatus->tio_status.DO_buffer & (1 << id)) > 0 ? 1 : 0;
    }
    else if (type == 2)
    {
        if (zucmotStatus->extio_stat.status != 1)
        {  // extio not running
            return -1;
        }
        if (id >= (int)sizeof(zucmotStatus->extio_stat.extDO))
        {
            return -1;
        }
        *status = zucmotStatus->extio_stat.extDO[id];
    }
    else if (type == 3)
    {
        if (id >= 2)
        {
            return -1;
        }
        *status = 0;
    }
    else if (type == 4)
    {
        if (id >= (int)sizeof(zucmotStatus->synch_mb_slave_do))
        {
            return -1;
        }
        *status = zucmotStatus->synch_mb_slave_do[id];
    }
    else if (type == 5)
    {
        if (id >= (int)sizeof(zucmotStatus->synch_pn_dev_do))
        {
            return -1;
        }
        *status = zucmotStatus->synch_pn_dev_do[id];
    }
    else if (type == 6)
    {
        if (zucmotStatus->eip_mode == 1 && id < zucmotStatus->eip_do_num)
        {
            int byteNum = id / 8;
            id -= byteNum * 8;
            *status = 0x01 & (0xff & (zucmotStatus->synch_eip_out[byteNum] >> id));
        }
        else if (zucmotStatus->eip_mode == 0 && id < (int)sizeof(zucmotStatus->synch_eip_adpt_do))
        {
            *status = zucmotStatus->synch_eip_adpt_do[id];
        }
        else
        {
            return -1;
        }
    }

    return 0;
}

int usrmotGetAIn(int type, int id, double* aval)
{
    if (zucmotStatus == 0)
    {
        usrmotInit("zuc_task");
    }
    if (id < 0)
        return -1;
    if (type == 0)
    {
        if (id >= (int)sizeof(zucmotStatus->analog_input))
            return -1;
        *aval = zucmotStatus->analog_input[id];
    }
    else if (type == 1)
    {
        if (id >= (int)sizeof(zucmotStatus->tio_status.AI))
            return -1;
        *aval = zucmotStatus->tio_status.AI[id];
    }
    else if (type == 2)
    {
        if (zucmotStatus->extio_stat.status != 1)
        {  // extio not running
            return -1;
        }
        if (id >= (int)sizeof(zucmotStatus->extio_stat.extAI))
            return -1;
        *aval = zucmotStatus->extio_stat.extAI[id];
    }
    else if (type == 4)
    {
        if (id >= (int)sizeof(zucmotStatus->analog_mb_slave_input))
            return -1;
        *aval = zucmotStatus->analog_mb_slave_input[id];
    }
    else if (type == 5)
    {
        if (id >= (int)sizeof(zucmotStatus->analog_pn_dev_input))
            return -1;
        *aval = zucmotStatus->analog_pn_dev_input[id];
    }
    else if (type == 6)
    {
        if (zucmotStatus->eip_mode == 1 && id < (zucmotStatus->eip_ai_int_num + zucmotStatus->eip_ai_float_num))
        {
            if (id < zucmotStatus->eip_ai_int_num)
            {
                union UN_INT32 un_int32;
                int byteNum = zucmotStatus->eip_di_num / 8;  //DI和DO数量必须为8的n次方。
                id *= 4;
                un_int32.PN_UI8[0] = zucmotStatus->synch_eip_in[byteNum + id + 0];
                un_int32.PN_UI8[1] = zucmotStatus->synch_eip_in[byteNum + id + 1];
                un_int32.PN_UI8[2] = zucmotStatus->synch_eip_in[byteNum + id + 2];
                un_int32.PN_UI8[3] = zucmotStatus->synch_eip_in[byteNum + id + 3];
                *aval = un_int32.PN_IN32;
            }
            else
            {
                union UN_Float union_float;
                int byteNum = zucmotStatus->eip_di_num / 8;
                id *= 4;
                union_float.PN_UI8[0] = zucmotStatus->synch_eip_in[byteNum + id + 0];
                union_float.PN_UI8[1] = zucmotStatus->synch_eip_in[byteNum + id + 1];
                union_float.PN_UI8[2] = zucmotStatus->synch_eip_in[byteNum + id + 2];
                union_float.PN_UI8[3] = zucmotStatus->synch_eip_in[byteNum + id + 3];
                *aval = union_float.PN_FLT;
            }
        }
        else if (zucmotStatus->eip_mode == 0 && id < (int)sizeof(zucmotStatus->analog_eip_adpt_input))
        {
            *aval = zucmotStatus->analog_eip_adpt_input[id];
        }
        else
        {
            return -1;
        }
    }

    return 0;
}

int usrmotGetAOut(int type, int id, double* aval)
{
    if (zucmotStatus == 0)
    {
        usrmotInit("zuc_task");
    }
    if (type == 0)
    {
        if (id >= (int)sizeof(zucmotStatus->analog_output))
            return -1;
        *aval = zucmotStatus->analog_output[id];
    }
    else if (type == 1)
    {
        if (id >= (int)sizeof(zucmotStatus->tio_status.AO))
            return -1;
        *aval = zucmotStatus->tio_status.AO[id];
    }
    else if (type == 2)
    {
        if (zucmotStatus->extio_stat.status != 1)
        {  // extio not running
            return -1;
        }
        if (id >= (int)sizeof(zucmotStatus->extio_stat.extAO))
            return -1;
        *aval = zucmotStatus->extio_stat.extAO[id];
    }
    else if (type == 4)
    {
        if (id >= (int)sizeof(zucmotStatus->analog_mb_slave_output))
            return -1;
        *aval = zucmotStatus->analog_mb_slave_output[id];
    }
    else if (type == 5)
    {
        if (id >= (int)sizeof(zucmotStatus->analog_pn_dev_output))
            return -1;
        *aval = zucmotStatus->analog_pn_dev_output[id];
    }
    else if (type == 6)
    {
        if (zucmotStatus->eip_mode == 1 && id < (zucmotStatus->eip_ao_int_num + zucmotStatus->eip_ao_float_num))
        {
            if (id < zucmotStatus->eip_ao_int_num)
            {
                union UN_INT32 un_int32;
                int byteNum = zucmotStatus->eip_do_num / 8;
                id *= 4;
                un_int32.PN_UI8[0] = zucmotStatus->synch_eip_out[byteNum + id + 0];
                un_int32.PN_UI8[1] = zucmotStatus->synch_eip_out[byteNum + id + 1];
                un_int32.PN_UI8[2] = zucmotStatus->synch_eip_out[byteNum + id + 2];
                un_int32.PN_UI8[3] = zucmotStatus->synch_eip_out[byteNum + id + 3];
                *aval = un_int32.PN_IN32;
            }
            else
            {
                union UN_Float union_float;
                int byteNum = zucmotStatus->eip_do_num / 8;  //DI和DO数量必须为8的n次方。
                id *= 4;
                union_float.PN_UI8[0] = zucmotStatus->synch_eip_out[byteNum + id + 0];
                union_float.PN_UI8[1] = zucmotStatus->synch_eip_out[byteNum + id + 1];
                union_float.PN_UI8[2] = zucmotStatus->synch_eip_out[byteNum + id + 2];
                union_float.PN_UI8[3] = zucmotStatus->synch_eip_out[byteNum + id + 3];
                *aval = union_float.PN_FLT;
            }
        }
        else if (zucmotStatus->eip_mode == 0 && id < (int)sizeof(zucmotStatus->analog_eip_adpt_output))
        {
            *aval = zucmotStatus->analog_eip_adpt_output[id];
        }
        else
        {
            return -1;
        }
    }

    return 0;
}

int usrmotGetAtlJointPose(double* jointPos)
{
    if (zucmotStatus == 0)
    {
        usrmotInit("zuc_task");
    }

    try
    {
        for (int i = 0; i < 6; i++) { jointPos[i] = zucmotStatus->joint_status[i].pos_cmd; }
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int usrmotGetAtlTcpPose(double* tcpPos)
{
    if (zucmotStatus == 0)
    {
        usrmotInit("zuc_task");
    }

    try
    {
        tcpPos[0] = zucmotStatus->carte_pos_cmd.tran.x;
        tcpPos[1] = zucmotStatus->carte_pos_cmd.tran.y;
        tcpPos[2] = zucmotStatus->carte_pos_cmd.tran.z;
        tcpPos[3] = zucmotStatus->carte_pos_cmd.a;
        tcpPos[4] = zucmotStatus->carte_pos_cmd.b;
        tcpPos[5] = zucmotStatus->carte_pos_cmd.c;
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int usrmotGetTIOSignalValue(const char* sigName, unsigned int* value)
{
    if (zucmotStatus == 0)
    {
        usrmotInit("zuc_task");
    }

    try
    {
        for (int i = 0; i < MAX_TIO_SIG_NUM; i++)
        {
            if (strcmp(sigName, zucmotStatus->gripperState[i].sigName) == 0)
            {
                *value = zucmotStatus->gripperState[i].value;
                if (zucmotStatus->gripperState[i].prog_state_when_error &&
                    (zucmotStatus->gripperState[i].commu_state == 3 || zucmotStatus->gripperState[i].commu_state == 1))
                {
                    return 1;
                }
                return 0;
            }
        }
    }
    catch (...)
    {
        return -1;
    }
    printf("Error: no signal named: %s is defined\n", sigName);
    return -1;
}

int usrmotGetAtlFlangePose(double* flangePos)
{
    if (zucmotStatus == 0)
    {
        usrmotInit("zuc_task");
    }

    try
    {
        flangePos[0] = zucmotStatus->carte_pos_flange.tran.x;
        flangePos[1] = zucmotStatus->carte_pos_flange.tran.y;
        flangePos[2] = zucmotStatus->carte_pos_flange.tran.z;
        flangePos[3] = zucmotStatus->carte_pos_flange.a;
        flangePos[4] = zucmotStatus->carte_pos_flange.b;
        flangePos[5] = zucmotStatus->carte_pos_flange.c;
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int get_joint_num() { return zucmotConfig->numJoints; }

int usrmotGetToolOffsets(double* offsets)
{
    if (zucmotStatus == 0)
    {
        usrmotInit("zuc_task");
    }

    try
    {
        offsets[0] = zucmotConfig->tool_offset.tran.x;
        offsets[1] = zucmotConfig->tool_offset.tran.y;
        offsets[2] = zucmotConfig->tool_offset.tran.z;
        offsets[3] = zucmotConfig->tool_offset.a;
        offsets[4] = zucmotConfig->tool_offset.b;
        offsets[5] = zucmotConfig->tool_offset.c;
        // rtapi_print("zucmotConfig->currentToolId = %d\n", zucmotConfig->currentToolId);
        // for(int i = 0; i < 6; i ++){
        // 	rtapi_print("offsets[%d] = %f\n", i, offsets[i]);
        // }
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int usrmotGetUserOffsets(double* offsets)
{
    if (zucmotStatus == 0)
    {
        usrmotInit("zuc_task");
    }

    try
    {
        offsets[0] = zucmotConfig->user_offset.tran.x;
        offsets[1] = zucmotConfig->user_offset.tran.y;
        offsets[2] = zucmotConfig->user_offset.tran.z;
        offsets[3] = zucmotConfig->user_offset.a;
        offsets[4] = zucmotConfig->user_offset.b;
        offsets[5] = zucmotConfig->user_offset.c;
        // rtapi_print("zucmotConfig->currentuserId = %d\n", zucmotConfig->currentuserId);
        // for(int i = 0; i < 6; i ++){
        // 	rtapi_print("offsets[%d] = %f\n", i, offsets[i]);
        // }
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int usrmotGetClsnSensitivity(int& clsn_sensitivity)
{
    if (zucmotStatus == 0)
    {
        usrmotInit("zuc_task");
    }

    try
    {
        clsn_sensitivity = zucmotConfig->safetyCfg.clsn_sensitivity;
        // rtapi_print("****[usrmotGetClsnSensitivity]]****clsn_sensitivity = %d\n", clsn_sensitivity);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int usrmotGetToolPayload(double* payload)
{
    if (zucmotStatus == 0)
    {
        usrmotInit("zuc_task");
    }

    try
    {
        payload[0] = zucmotStatus->payload.m;
        for (int i = 1; i < 4; i++) { payload[i] = zucmotStatus->payload.p[(i - 1)]; }
        // for(int i = 0; i < 4; i ++){
        // 	rtapi_print("****[usrmotGetToolPayload]****payload[%d] = %f\n", i, payload[i]);
        // }
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int usrmotGetSensorType(int type, double* sensor_value)
{
    if (zucmotStatus == 0)
    {
        usrmotInit("zuc_task");
    }

    if (type == 0 || type == 1)
    {
        for (int i = 0; i < 6; i++) { sensor_value[i] = zucmotStatus->sensor_stat.actTorque[i]; }
    }
    else
    {
        return -1;
    }
    return 0;
}

int usrmotSetEmergencyStopPaused(unsigned char val)
{
    zucmotStatus->category_one_stop_program_pause = val;
    return 0;
}
int usrmotGetEmergencyStopPaused() { return zucmotStatus->category_one_stop_program_pause; }

void usrmotSetSteppingState(int val) { zucmotDebug->stepping = val; }

int usrmotGetSteppingState() { return zucmotDebug->stepping; }